#include <stdio.h>
#include "esp_log.h"
#include "driver/i2c.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"

#include "tmI2c.h"
#include "tmModbus_tcp.h"

static const char *TAG = "i2c-simple-example";

#define I2C_MASTER_SCL_IO           5      /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           4      /*!< GPIO number used for I2C master data  */
#define I2C_MASTER_NUM              0                          /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ          10000                     /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       10000

#define AS5600_I2C_ADDR             0x36

//
uint8_t gAs5600Data[2]={0x10, 0x20};

// 
esp_err_t i2c_master_init(void)
{
    int i2c_master_port = I2C_MASTER_NUM;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ
    };

    i2c_param_config(i2c_master_port, &conf);

    return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}

void as5600_task(void *pvParameters)
{
    uint8_t data[20]={0};
    char pr[100]={0};
    esp_err_t ret;

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, AS5600_I2C_ADDR << 1 | I2C_MASTER_WRITE, false);
    i2c_master_write_byte(cmd, 0, false);
    i2c_master_write_byte(cmd, 0x04, false);
    i2c_master_stop(cmd);
    ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);

    while (1)
    {
        vTaskDelay(500 / portTICK_PERIOD_MS);
        // (as5600_register_read(MPU9250_WHO_AM_I_REG_ADDR, data, 1));

        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, AS5600_I2C_ADDR << 1 | I2C_MASTER_WRITE, false);
        i2c_master_write_byte(cmd, 0x0C, false);
        i2c_master_stop(cmd);
        ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);

        cmd = i2c_cmd_link_create();
        vTaskDelay(50 / portTICK_PERIOD_MS);
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, AS5600_I2C_ADDR << 1 | I2C_MASTER_READ, false);
        // i2c_master_read(cmd, data, 18, 0);
        i2c_master_read_byte(cmd, data, I2C_MASTER_ACK);
        i2c_master_read_byte(cmd, &data[1], I2C_MASTER_LAST_NACK);
        i2c_master_stop(cmd);
        ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);

        TmMbChargeAngleValue(data[0], data[1]);
        // for (int i = 0; i < 19; i++)
        //     sprintf(pr, "%x", data[i]);
        // ESP_LOGI(TAG, "%s", pr);
        // i2c_master_write_to_device(I2C_NUM_0, 0x1D, data, 1, 500 / portTICK_PERIOD_MS);
        // i2c_read(0x1D, 0x00, 1, data);
        // i2c_write(0x40)

        gAs5600Data[0] = data[0];
        gAs5600Data[1] = data[1];
        ESP_LOGI(TAG, "ret = %d, AS5600 = %X, %X", ret, data[0], data[1]);
    }
}

void GetAs5600Data(uint8_t *dataH, uint8_t *dataL)
{
    *dataH = gAs5600Data[0];
    *dataL = gAs5600Data[1];
}

static void display_task(void *pvParameters)
{
    uint8_t data[2]={0};
    esp_err_t ret;
    while (1)
    {
        vTaskDelay(500 / portTICK_PERIOD_MS);
        // (as5600_register_read(MPU9250_WHO_AM_I_REG_ADDR, data, 1));

        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        // i2c_master_write_byte(cmd, AS5600_I2C_ADDR << 1 | I2C_MASTER_WRITE, false);
        i2c_master_write_byte(cmd, 0x40, false);
        i2c_master_stop(cmd);
        ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);

        cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, 0xc0, false);
        // i2c_master_stop(cmd);
        // ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
        // i2c_cmd_link_delete(cmd);

        // cmd = i2c_cmd_link_create();
        // i2c_master_start(cmd);
        i2c_master_write_byte(cmd, 0x3f, false);
        i2c_master_write_byte(cmd, 0x06, false);
        i2c_master_write_byte(cmd, 0x3f, false);
        i2c_master_write_byte(cmd, 0x06, false);
        i2c_master_stop(cmd);
        ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);

        cmd = i2c_cmd_link_create();
        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, 0x8f, false);
        i2c_master_stop(cmd);
        ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
        i2c_cmd_link_delete(cmd);

        // cmd = i2c_cmd_link_create();
        // vTaskDelay(50 / portTICK_PERIOD_MS);
        // i2c_master_start(cmd);
        // i2c_master_write_byte(cmd, AS5600_I2C_ADDR << 1 | I2C_MASTER_READ, false);
        // i2c_master_read_byte(cmd, data, false);
            // i2c_master_stop(cmd);
        // ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);
        // i2c_cmd_link_delete(cmd);

        // i2c_master_write_to_device(I2C_NUM_0, 0x1D, data, 1, 500 / portTICK_PERIOD_MS);
        // i2c_read(0x1D, 0x00, 1, data);
        // i2c_write(0x40)

        ESP_LOGI(TAG, "WHO_AM_I = %X", data[0]);
    }
}
